
cdef extern from "math.h":
    float cosf(float theta)
    float sinf(float theta)
    float acosf(float theta)
    float sqrt(float theta)
    
PI = 3.1415926535897931

cdef class Vector3b:

    cdef public float w
    cdef public float x
    cdef public float y
    cdef public float z

    def __cinit__(self):
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0
        self.w = 0.0

    cpdef Vector3b set(self, float x, float y, float z, float w = 0.0):
        self.x = x
        self.y = y
        self.z = z
        return self

    # se reduce 21%
    def angle(Vector3b self, Vector3b other, unit='radian'):
        if(isinstance(other, Vector3b)):
            modOther = other.module()
            modSelf = self.module()
            #ningun denominador puede ser cero
            if not(modOther==0 or modSelf==0):
                numer = self.dot(other)
                #vectores ortogonales
                if(numer==0):
                    if(unit=='radian'):
                        return PI / 2.0
                    elif(unit=='grade'):
                        return 90
                denom = modOther * modSelf
                division = numer / denom
                try:
                    angle = acosf(division)
                except:
                    if(division - 1.0 < 1E-15):
                        angle = 0
                    elif(division + 1.0 < 1E-15):
                        angle = PI
                if(unit=='grade'):
                    angle = angle * 180.0 / PI
                return angle
            else:
                return None

    def proyection(Vector3b self, Vector3b other):
        proy = None
        if(isinstance(other, Vector3b)):
            proy = (self.dot(other))
            proy /= other.module()
        return proy


#    def dot(self, other):
#        assert isinstance(other, Vector3)
#        return self.x * other.x + \
#               self.y * other.y + \
#               self.z * other.z

    # se reduce 30%
    cpdef dot(Vector3b self, Vector3b other):
        #assert isinstance(other, Vector3b)
        return self.x * other.x +  self.y * other.y  +  self.z * other.z

#    def __abs__(self):
#        return math.sqrt(self.x ** 2 + \
#                         self.y ** 2 + \
#                         self.z ** 2)

    # se reduce 34%
    def __abs__(Vector3b self):
        return sqrt(self.w * self.w + \
                         self.x * self.x + \
                         self.y * self.y + \
                         self.z * self.z)

    magnitude = __abs__

    def magnitude_squared(self):
        return self.x * self.x + self.y * self.y + self.z * self.z

    def __sub__(self, other):
        cdef Vector3b res = Vector3b()
        if isinstance(other, Vector3b):
            res.x = self.x - other.x
            res.y = self.y - other.y
            res.z = self.z - other.z
            return res

    def __mul__(Vector3b self, other):
        cdef Vector3b res = Vector3b()
        if isinstance(other, Vector3b):
            res.x = self.x * other.x
            res.y = self.y * other.y
            res.z = self.z * other.z
            return res
        else: 
            assert type(other) in (int, long, float)
            res.x = self.x * other
            res.y = self.y * other
            res.z = self.z * other
            return res
        

    cpdef cross(Vector3b self, Vector3b other):
        cdef Vector3b res = Vector3b()
        res.x = self.y * other.z - self.z * other.y
        res.y = -self.x * other.z + self.z * other.x
        res.z = self.x * other.y - self.y * other.x
        return res

    cpdef normalize(self):
        d = self.magnitude()
        if d:
            self.x /= d
            self.y /= d
            self.z /= d
        return self

    cpdef module(self):
        return sqrt(self.magnitude_squared())

    def __add__(Vector3b self, Vector3b other):
       cdef Vector3b res = Vector3b()
       res.x = self.x + other.x
       res.y = self.y + other.y
       res.z = self.z + other.z
       return res

    def printSelf(self, titulo = ''):
        print titulo, self.x, self.y, self.z

    cpdef Vector3b rotate(self, ang, eje, tolerance=0.01):
        #rota con respecto a un eje, angulo en radianes
        p = eje * (self.dot(eje) / eje.magnitude_squared())
        d = self - p
        norm = (eje.cross(self)).normalize()
        md = d.module()
        if(md<tolerance):
            return self
        else:
            return p+d*cosf(ang)+norm*(d.module()*sinf(ang))

    def __copy__(self):
        return self.__class__(self.x, self.y, self.z)

    copy = __copy__

    def __repr__(self):
        return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
                                              self.y,
                                              self.z)

#    Error: must be implemented via __richcmp__
#    def __eq__(self, other, tolerance=0):
#        #Params: other (Vector3|[x,y,z]), tolerance float
#        if isinstance(other, Vector3b):
#            return (self - other).module() <= tolerance

    def __len__(self):
        return 3

    def __neg__(self):
       cdef Vector3b res = Vector3b()
       res.x = -self.x
       res.y = -self.y
       res.z = -self.z
       return res

#    gush
    cpdef inRect(Vector3b self, recta, tolerance=1e-3):
        p = recta[0]
        v = p - recta[1]
        q = self
        pq = p-q
        dist = pq.dot(v)/v.module()
        dist = (pq.cross(v)).module()/v.module()
        return dist <= tolerance

#    cpdef Vector3b set(self, float x, float y, float z, float w = 0.0):
#        self.x = x
#        self.y = y
#        self.z = z
#        return self
'''
#cdef Vector3 V = Vector3()
#V.set(1,2,3)
#print V.x
'''

''''
class Vector3:
#    __slots__ = ['x', 'y', 'z']

#    def __init__(self, x, y, z):
#        self.x = x
#        self.y = y
#        self.z = z

#    def rotate2(self, ang, eje, tol = 0.01):
#        return

    def rotate(self, ang, eje, tolerance=0.01):
         #rota con respecto a un eje, angulo en radianes
        p = eje * (self.dot(eje)/eje.magnitude_squared())
        d = self - p
        norm = (eje.cross(self)).normalize()
        md = d.module()
        if(md<tolerance):
            return self
        else:
            return p+d*math.cos(ang)+norm*(d.module()*math.sin(ang))

    def angle(self, other, unit='radian'):
        if(isinstance(other, Vector3)):
#            print 'other', other
            modOther = other.module()
#            print 'modOther', modOther
            modSelf = self.module()
#            print 'self', self
#            print 'modSelf', modSelf
            #ningun denominador puede ser cero
            if not(modOther==0 or modSelf==0):
                numer = self.dot(other)
#                print 'numer', numer
                #vectores ortogonales
                if(numer==0):
                    if(unit=='radian'):
                        return math.pi/2
                    elif(unit=='grade'):
                        return 90
                denom = modOther * modSelf
#                print 'denom', denom
                division = numer / denom
#                angle = math.acos(division)
                try:
                    angle = math.acos(division)
                except:
#                    print 'except division', (division - 1.0 < 1E-15), division, numer, denom
                    if(division - 1.0 < 1E-15):
                        angle = 0
                    elif(division + 1.0 < 1E-15):
                        angle = math.pi
##                print 'division', division
                
                if(unit=='grade'):
                    angle = math.degrees(angle)
#                print 'angulo', angle
                return angle
            else:
#                print 'modulo cero'
                return None

    def proyection(self, other):
        proy = None
        if(isinstance(other, Vector3)):
            proy = (self.dot(other))
            proy /= other.module()
        return proy

    def __copy__(self):
        return self.__class__(self.x, self.y, self.z)

    copy = __copy__

    def __repr__(self):
        return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
                                              self.y,
                                              self.z)



    def __eq__(self, other, tolerance=0):
        #Params: other (Vector3|[x,y,z]), tolerance float
        if isinstance(other, Vector3):
            return (self - other).module() <= tolerance
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return (self - other).module() <= tolerance

    def __ne__(self, other):
        return not self.__eq__(other)

    def __nonzero__(self):
        return self.x != 0 or self.y != 0 or self.z != 0

    def __len__(self):
        return 3

    def __getitem__(self, key):
        return (self.x, self.y, self.z)[key]

    def __setitem__(self, key, value):
        l = [self.x, self.y, self.z]
        l[key] = value
        self.x, self.y, self.z = l

    def __iter__(self):
        return iter((self.x, self.y, self.z))

    def __getattr__(self, name):
        try:
            return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \
                          for c in name])
        except ValueError:
            raise AttributeError, name

    if _enable_swizzle_set:
        # This has detrimental performance on ordinary setattr as well
        # if enabled
        def __setattr__(self, name, value):
            if len(name) == 1:
                object.__setattr__(self, name, value)
            else:
                try:
                    l = [self.x, self.y, self.z]
                    for c, v in map(None, name, value):
                        l['xyz'.index(c)] = v
                    self.x, self.y, self.z = l
                except ValueError:
                    raise AttributeError, name


    def __add__(self, other):
        if isinstance(other, Vector3):
            # Vector + Vector -> Vector
            # Vector + Point -> Point
            # Point + Point -> Vector
            if self.__class__ is other.__class__:
                _class = Vector3
            else:
                _class = Point3
            return _class(self.x + other.x,
                          self.y + other.y,
                          self.z + other.z)
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return Vector3(self.x + other[0],
                           self.y + other[1],
                           self.z + other[2])
    __radd__ = __add__

    def __iadd__(self, other):
        if isinstance(other, Vector3):
            self.x += other.x
            self.y += other.y
            self.z += other.z
        else:
            self.x += other[0]
            self.y += other[1]
            self.z += other[2]
        return self

    def __sub__(self, other):
        if isinstance(other, Vector3):
            # Vector - Vector -> Vector
            # Vector - Point -> Point
            # Point - Point -> Vector
            if self.__class__ is other.__class__:
                _class = Vector3
            else:
                _class = Point3
            return Vector3(self.x - other.x,
                           self.y - other.y,
                           self.z - other.z)
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return Vector3(self.x - other[0],
                           self.y - other[1],
                           self.z - other[2])

   
    def __rsub__(self, other):
        if isinstance(other, Vector3):
            return Vector3(other.x - self.x,
                           other.y - self.y,
                           other.z - self.z)
        else:
            assert hasattr(other, '__len__') and len(other) == 3
            return Vector3(other.x - self[0],
                           other.y - self[1],
                           other.z - self[2])

    def __mul__(self, other):
        if isinstance(other, Vector3):
            # TODO component-wise mul/div in-place and on Vector2; docs.
            if self.__class__ is Point3 or other.__class__ is Point3:
                _class = Point3
            else:
                _class = Vector3
            return _class(self.x * other.x,
                          self.y * other.y,
                          self.z * other.z)
        else: 
            assert type(other) in (int, long, float)
            return Vector3(self.x * other,
                           self.y * other,
                           self.z * other)

    __rmul__ = __mul__

    def __imul__(self, other):
        assert type(other) in (int, long, float)
        self.x *= other
        self.y *= other
        self.z *= other
        return self

    def __div__(self, other):
        assert type(other) in (int, long, float)
        return Vector3(operator.div(self.x, other),
                       operator.div(self.y, other),
                       operator.div(self.z, other))


    def __rdiv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3(operator.div(other, self.x),
                       operator.div(other, self.y),
                       operator.div(other, self.z))

    def __floordiv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3(operator.floordiv(self.x, other),
                       operator.floordiv(self.y, other),
                       operator.floordiv(self.z, other))


    def __rfloordiv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3(operator.floordiv(other, self.x),
                       operator.floordiv(other, self.y),
                       operator.floordiv(other, self.z))

    def __truediv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3(operator.truediv(self.x, other),
                       operator.truediv(self.y, other),
                       operator.truediv(self.z, other))


    def __rtruediv__(self, other):
        assert type(other) in (int, long, float)
        return Vector3(operator.truediv(other, self.x),
                       operator.truediv(other, self.y),
                       operator.truediv(other, self.z))
    
    def __neg__(self):
        return Vector3(-self.x,
                        -self.y,
                        -self.z)

    __pos__ = __copy__
    
    def __abs__(self):
        return math.sqrt(self.x ** 2 + \
                         self.y ** 2 + \
                         self.z ** 2)

    magnitude = __abs__
    
    #gush
    def inRect(self, recta, tolerance=1e-3):
        p = recta[0]
        v = p - recta[1]
        q = self
        pq = p-q
        dist = pq.dot(v)/v.module()
        dist = (pq.cross(v)).module()/v.module()
        return dist <= tolerance

    def magnitude_squared(self):
        return self.x ** 2 + \
               self.y ** 2 + \
               self.z ** 2

    def module(self):
        return math.sqrt(self.magnitude_squared())

    def normalize(self):
        d = self.magnitude()
        if d:
            self.x /= d
            self.y /= d
            self.z /= d
        return self

    def normalized(self):
        d = self.magnitude()
        if d:
            return Vector3(self.x / d, 
                           self.y / d, 
                           self.z / d)
        return self.copy()

    def dot(self, other):
        assert isinstance(other, Vector3)
        return self.x * other.x + \
               self.y * other.y + \
               self.z * other.z

    def cross(self, other):
        assert isinstance(other, Vector3)
        return Vector3(self.y * other.z - self.z * other.y,
                       -self.x * other.z + self.z * other.x,
                       self.x * other.y - self.y * other.x)

    def reflect(self, normal):
        # assume normal is normalized
        assert isinstance(normal, Vector3)
        d = 2 * (self.x * normal.x + self.y * normal.y + self.z * normal.z)
        return Vector3(self.x - d * normal.x,
                       self.y - d * normal.y,
                       self.z - d * normal.z)
'''
